package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Timer;
import java.util.TimerTask;

public class RateEncoder extends Encoder {
    private double m_maxSpeed;
    private double m_minSpeed;
    private double m_prevDistance = 0.0;
    private double m_rate;
    private Timer m_timer = new Timer();
    private java.util.Timer m_timer2 = new java.util.Timer();

    public RateEncoder(int firstChannel, int secondChannel, double maxSpeed, double minSpeed, double period) {
        super(firstChannel, secondChannel);
        m_maxSpeed = maxSpeed;
        m_minSpeed = minSpeed;
        m_timer.reset();
        m_timer.start();
        m_timer2.schedule(new TimerTask() {
            public void run() {
                m_rate = Math.min(Math.max((getDistance() - m_prevDistance) / m_timer.get(), m_minSpeed), m_maxSpeed);
                m_prevDistance = getDistance();
                m_timer.reset();
            }
        }, 0, (int) (period * 1000));
    }

    public double getRate() {
        return m_rate;
    }
}